// $Id: oaxaca-rpi.c 416 2014-03-07 22:40:52Z alan.watson.f $

#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <sys/time.h>
#include <errno.h>
#include <stdint.h>
#include <unistd.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <netinet/tcp.h>
#include <stddef.h>
#include <sched.h>
#include <sys/mman.h>

#include "bcm2835.h"

#if OAXACA_LAMP
#define USE_TCP 1
#endif

////////////////////////////////////////////////////////////////////////////////

#if OAXACA_DM

// The DM is driven by a pair of AD5535s producing a total of 64
// channels of 0 V to +200 V.

const unsigned int nchannel         = 64;
const unsigned int dacdatamax       = 0x3fff;
const unsigned int dacdelay         = 0;
const unsigned int dacstartinterval = 100000;

const unsigned int pin = 0;

const char *servername = "dm";
const int serverport = 5000;

#elif OAXACA_TM

// The TM is driven by a single AD5372 with a 3 V reference. By default,
// it produces 32 channels of -4 V to +8 V. However, we only use 3
// channels. We also set the offset registers (in dacstart) to give 0 V
// to +12 V. Since, we only want 0 V to +10 V, so we reduce dacdatamax
// proportionately.

const unsigned int nchannel         = 3;
const unsigned int dacdatamax       = 0xffff;
const unsigned int dacdelay         = 2000;
const unsigned int dacstartinterval = 100000;

const unsigned int pin = 0;

const char *servername = "tm";
const int serverport = 5001;

#elif OAXACA_LAMP

// The lamp is controlled by P1-13 (GPIO 27 on our revision 2 boards).

const unsigned int nchannel         = 0;
const unsigned int dacdatamax       = 0;
const unsigned int dacdelay         = 0;
const unsigned int dacstartinterval = 0;

const unsigned int pin = RPI_V2_GPIO_P1_13;

const char *servername = "lamp";
const int serverport = 5002;

#endif

// This array defines the mapping from Adaptica channel numbers (1 to
// 64) to the slave (0 or 1) and address (0 to 31) of the corresponding
// DACs. For example, for the DM, channel 20 corresponds to
// channel 2 of slave 0.

static struct {
  int slave;
  unsigned int address;
} channelmap[] = {
#if OAXACA_DM
  { 0, 27 },  // Channel 1
  { 0, 17 },
  { 0, 30 },
  { 0, 20 },
  { 0, 31 },
  { 0, 19 },
  { 0, 23 },
  { 0, 22 },
  { 0, 26 },
  { 0, 29 }, // Channel 10
  { 0, 25 },
  { 0, 24 },
  { 0, 21 },
  { 0, 18 },
  { 0, 14 },
  { 0, 28 },
  { 0, 16 },
  { 0,  6 },
  { 0, 13 },
  { 0,  2 }, // Channel 20
  { 0, 12 },
  { 0,  3 },
  { 0,  7 },
  { 0,  1 },
  { 0,  4 },
  { 0,  8 },
  { 0,  0 },
  { 0,  9 },
  { 0, 10 },
  { 0, 11 }, // Channel 30
  { 0,  5 },
  { 0, 15 },
  { 1, 27 },
  { 1, 17 },
  { 1, 30 },
  { 1, 20 },
  { 1, 31 },
  { 1, 19 },
  { 1, 23 },
  { 1, 22 }, // Channel 40
  { 1, 26 },
  { 1, 29 },
  { 1, 25 },
  { 1, 24 },
  { 1, 21 },
  { 1, 18 },
  { 1, 14 },
  { 1, 28 },
  { 1, 16 },
  { 1,  6 }, // Channel 50
  { 1, 13 },
  { 1,  2 },
  { 1, 12 },
  { 1,  3 },
  { 1,  7 },
  { 1,  1 },
  { 1,  4 },
  { 1,  8 },
  { 1,  0 },
  { 1,  9 }, // Channel 60
  { 1, 10 },
  { 1, 11 },
  { 1,  5 },
  { 1, 15 },
#elif OAXACA_TM
  // The TM is connected to VOUT 20, 21, and 22, which correspond to
  // DACs 4, 5, and 6 of group 2.
  { 0, 034 },
  { 0, 035 },
  { 0, 036 },
#endif
};

////////////////////////////////////////////////////////////////////////////////

void
vfatalerror(const char *s, va_list ap)
{
  fprintf(stderr, "fatal error: ");
  vfprintf(stderr, s, ap);
  fprintf(stderr, "\n");
  exit(1);
}

void 
fatalerror(const char *s, ...)
{
  va_list ap;
  va_start(ap, s);
  vfatalerror(s, ap);
}

////////////////////////////////////////////////////////////////////////////////

static double
seconds(void)
{
  struct timeval tv;
  gettimeofday(&tv, NULL);
  return tv.tv_sec + 1e-6 * tv.tv_usec;
}

__attribute__((unused))
static void
waitmicroseconds(unsigned long delay)
{
  // Wait for delay microseconds.
  
  // Note that nanosleep takes typically 70-90 µs with real-time
  // scheduling and 120-140 µs with normal scheduling. Occasionally, it
  // takes much longer (200-400 µs). If we need to delay for less time,
  // one option is a busy loop on the BCM 2835's ST clock. See the code
  // in bcm2835.c.
  
  if (delay > 0) {
    struct timespec ts;
    ts.tv_sec = delay / 1000000ul;
    ts.tv_nsec = (delay % 1000000ul) * 1000ul;
    errno = 0;
    while (nanosleep(&ts, &ts) == -1 && errno == EINTR)
      errno = 0;
  }
}

////////////////////////////////////////////////////////////////////////////////

void
gpiostart(void)
{

  // Set highest real-time priority, FIFO scheduling, and avoid swapping.
  // See https://groups.google.com/d/msg/bcm2835/FmQySCZCntw/Fc9ewmwZWzAJ

  struct sched_param sp;
  memset(&sp, 0, sizeof(sp));
  sp.sched_priority = sched_get_priority_max(SCHED_FIFO);
  sched_setscheduler(0, SCHED_FIFO, &sp);
  mlockall(MCL_CURRENT | MCL_FUTURE);

  // Initialize the bcm2835 library.

  if (!bcm2835_init())
    fatalerror("bcm2835_init() failed.");
    
#if OAXACA_TM || OAXACA_DM

 // Prepare for SPI transfers.

  bcm2835_spi_begin();
  bcm2835_spi_setBitOrder(BCM2835_SPI_BIT_ORDER_MSBFIRST);
  bcm2835_spi_setDataMode(BCM2835_SPI_MODE1);
  bcm2835SPIClockDivider divider = BCM2835_SPI_CLOCK_DIVIDER_32;
  bcm2835_spi_setClockDivider(divider);  
  bcm2835_spi_chipSelect(BCM2835_SPI_CS0);
  bcm2835_spi_setChipSelectPolarity(BCM2835_SPI_CS0, LOW);
  
#elif OAXACA_LAMP

  // Set the pin to be an output.  
  bcm2835_gpio_fsel(pin, BCM2835_GPIO_FSEL_OUTP);
  
#endif

}

void
gpioend(void)
{
  // Finalize the bcm2835 library.

#if OAXACA_TM || OAXACA_DM
  bcm2835_spi_end();
#endif
  
  bcm2835_close();

}

void
spiwritebits(int i, size_t n, unsigned long u)
{
  //printf("spiwritebits %d %lu %08lx\n", i, (unsigned long) n, u);

  // Write the lower n bits of u to the SPI slave i.

  // Select the SPI slave.

  if (i == 0)
    bcm2835_spi_chipSelect(BCM2835_SPI_CS0);
  else if (i == 1)
    bcm2835_spi_chipSelect(BCM2835_SPI_CS1);
  else
    fatalerror("invalid slave \"%d\".", i);

  // To write n bits, we need to write m bytes.

  size_t m = (n + 7) / 8;

  // Shift so that bit n is now bit m * 8.

  u <<= m * 8 - n;

  // Extracts the lower m bytes.

  uint8_t b[m];
  for (size_t i = 0; i < m; ++i)
    b[i] = (u >> ((m - i - 1) * 8)) & 0xff;

  // Write the m bytes to the selected SPI slave.

  bcm2835_spi_writenb((char *) b, m);

}

////////////////////////////////////////////////////////////////////////////////

static void
dacwritedirect(unsigned int slave, unsigned int address, unsigned long data)
{
#if OAXACA_DM
  if (slave > 1)
    fatalerror("invalid slave \"%lu\".", slave);
  if (address > 0x1f)
    fatalerror("invalid address \"0x%lx\".", address);
  if (data > 0x3fff)
    fatalerror("invalid data \"0x%lx\".", data);
  unsigned long bits = ((unsigned long) address << 14) | data;
  spiwritebits(slave, 19, bits);
#elif OAXACA_TM
  if (slave != 0)
    fatalerror("invalid slave \"%lu\".", slave);
  if ((address & 070) == 5 || address == 005 || address == 006 || address == 007)
    fatalerror("invalid address \"0x%lx\".", address);
  if (data > 0xffff)
    fatalerror("invalid data \"0x%lx\".", data);
  unsigned long mode = 3; // Write to X register
  unsigned long bits = (mode << 22) | ((unsigned long) address << 16) | data;
  spiwritebits(slave, 24, bits);
#endif
}

static void
dacwritechannel(unsigned int channel, unsigned long data)
{
  if (channel > nchannel)
    fatalerror("invalid channel \"%lu\".", channel);
  dacwritedirect(channelmap[channel].slave, channelmap[channel].address, data);
}

static void
dacwriteallchannels(unsigned long data)
{
  for (unsigned int channel = 0; channel < nchannel; ++channel)
    dacwritechannel(channel, data);
}

void
dacstart(void)
{
#if OAXACA_TM

  // The TM is driven by a single AD5372 with a 3 V reference. By
  // default, it produces -4 V to +8 V, but we want 0 V to +10 V. We set
  // the OSF0 and OSF1 registers to give 0 V to +12 V. Then we set the M
  // registers to give 0 V to +10 V.

  // Set the OFS0 and OFS1 registers.

  spiwritebits(0, 24, 0x020000);
  spiwritebits(0, 24, 0x030000);
  
  // Set the M registers (mode = 0x1, address = 0x0, and data = (10.0/12.0)
  // * 0xffff), then allow 1 µs per DAC for the updates.
  
  unsigned long bits = (0x1ul << 22) | (unsigned long) (10.0 / 12.0 * (double) 0xffff);
  spiwritebits(0, 24, bits);
  waitmicroseconds(32);

  // Set all DACs to 0 (mode = 0x3, address = 0x0, and data = 0x0), then
  // allow 1 µs per DAC for the updates.

  spiwritebits(0, 24, 0xc00000);
  waitmicroseconds(32);

#endif

  // Set all channels to 0 V.
  
  dacwriteallchannels(0);
}

void
dacend(void)
{
  dacwriteallchannels(0);
}

void
dacsquarewave(unsigned long n)
{
  // Drive the DACs with a square wave as fast as allowed for n updates.

  double start = seconds();
    
  for (unsigned long i = 0; i < n; ++i) {
#if OAXACA_DM
    if (i % 2 == 0) {
      for (unsigned int channel = 0; channel < nchannel; ++channel) {
        dacwritechannel(channel, channel < 32 ? 0 : dacdatamax);
        waitmicroseconds(dacdelay);
      }
      printf("in\n");
    } else {
      for (unsigned int channel = 0; channel < nchannel; ++channel) {
        dacwritechannel(channel, channel < 32 ? dacdatamax : 0);
        waitmicroseconds(dacdelay);
      }
      printf("out\n");
    }
#elif OAXACA_TM
    unsigned int channel = 2;
    if (i % 2 == 0) {
      for (int x = 0; x <= 10; ++x) {
        dacwritechannel(channel, 0.1 * x * dacdatamax);
        waitmicroseconds(dacdelay);
      }
    } else {
      for (int x = 0; x <= 10; ++x) {
        dacwritechannel(channel, 0.1 * (10 - x) * dacdatamax);
        waitmicroseconds(dacdelay);
      }
    }
#endif
  }
    
  double end = seconds();
    
  double t = (end - start) / (double) n / (double) nchannel;
  printf("Each DAC %.1f kHz %.1f µs\n", 1e-3 / t, t * 1e6);
  printf("Set of %u DACs %.1f kHz %.1f µs\n", nchannel, 1e-3 / t / nchannel, t * nchannel * 1e6);

}

////////////////////////////////////////////////////////////////////////////////

void
lampwrite(int status)
{
  // Switch the lamp on or off.
  
  bcm2835_gpio_write(pin, status ? HIGH : LOW);
   
  // Fernando suggests a 5 second delay.
  
  sleep(5);
}

////////////////////////////////////////////////////////////////////////////////

void
drive(char *line)
{
  printf("line: %s", line);

  char command;
    
  if (sscanf(line, "%c", &command) != 1)
    return;
        
#if OAXACA_TM || OAXACA_DM

  if (command == 'a') {

    unsigned long data;
    if (sscanf(line, "%*c %lx", &data) != 1)
      fatalerror("in command '%c': incorrect data from client.", command);
    dacwriteallchannels(data);

  } else if (command == 'c') {

    unsigned long channel;
    unsigned long data;
    if (sscanf(line, "%*c %lu %lx", &channel, &data) != 2)
      fatalerror("in command '%c': incorrect data from client.", command);
    dacwritechannel(channel, data);

  } else if (command == 'd') {

    int slave;
    int address;
    unsigned long data;
      
    if (sscanf(line, "%*c %d %d %lx", &slave, &address, &data) != 3)
      fatalerror("in command '%c': incorrect data from client.", command);
    dacwritedirect(slave, address, data);
      
  } else if (command == 'm') {

    char *s = line + 1;
    unsigned long channel;
    unsigned long data;      
    for (channel = 0; channel < nchannel; ++channel) {
      errno = 0;
      data = strtoul(s, &s, 16);
      if (errno != 0)
        fatalerror("in command '%c': incorrect data from client.", command);
      dacwritechannel(channel, data);
    }

  } else if (command == 's') {

      double n;
      if (sscanf(line, "%*c %lf", &n) != 1)
        fatalerror("in command '%c': incorrect data from client.", command);
      dacsquarewave(n);    
   } else if (command != 'q') {
     fatalerror("incorrect command from client.");

  }
     
#elif OAXACA_LAMP

  if (command == 'l') {
    
    int state;
    if (sscanf(line, "%*c %d", &state) != 1)
      fatalerror("in command '%c': incorrect data from client.", command);
    lampwrite(state);

  } else if (command != 'q') {

    fatalerror("incorrect command from client.");

  }
    
#endif
    
  if (command == 'q') {
#if OAXACA_TM || OAXACA_DM
    dacend();
#endif
    gpioend();
    exit(0);
  }
      
}

void
drivestream(FILE *in, FILE *out)
{
  unsigned long sequence = 0;
  char *line = NULL;
  size_t linecap = 0;
  ssize_t linelen;
  while ((linelen = getline(&line, &linecap, in)) > 0) {
    drive(line);
    fprintf(out, "%lu\n", sequence);
    ++sequence;
    printf("reply: %lu\n", sequence);
  }
}

void
local(void)
{
  drivestream(stdin, stdout);
}

void
server(void)
{

#if USE_TCP

  int listenfd = socket(AF_INET, SOCK_STREAM, 0);
  if (listenfd < 0)
    fatalerror("socket failed.");

  struct sockaddr_in sa;
  memset(&sa, 0, sizeof(sa));
  sa.sin_family = AF_INET;
  sa.sin_addr.s_addr = htonl(INADDR_ANY);
  sa.sin_port = htons(serverport);
  errno = 0;
  if (bind(listenfd, (struct sockaddr *) &sa, sizeof(sa)) < 0)
    fatalerror("bind failed: %s.", strerror(errno));
    
  if (listen(listenfd, 10) < 0)
    fatalerror("listen failed.");
      
  printf("waiting for connection.\n");
  
  for (;;) {

    int fd = accept(listenfd, NULL, NULL);
    if (fd < 0)
      fatalerror("accept failed.");

    int flag = 1;
    int result = setsockopt(listenfd, IPPROTO_TCP, TCP_NODELAY, (char *) &flag, sizeof(int));
    if (result < 0)
      fatalerror("setsockopt failed.");

    printf("connected.\n");
  
    FILE *in = fdopen(fd, "r");
    FILE *out = fdopen(fd, "w");
    setvbuf(out, NULL, _IOLBF, 0);
    
    drivestream(in, out);  
    
    fclose(in);
    fclose(out);

  }
  
#else  

  int fd = socket(AF_INET, SOCK_DGRAM, 0);
  if (fd < 0)
    fatalerror("socket failed.");

  struct sockaddr_in sa;
  memset(&sa, 0, sizeof(sa));
  sa.sin_family = AF_INET;
  sa.sin_addr.s_addr = htonl(INADDR_ANY);
  sa.sin_port = htons(serverport);
  errno = 0;
  if (bind(fd, (struct sockaddr *) &sa, sizeof(sa)) < 0)
    fatalerror("bind failed: %s.", strerror(errno));
      
  for (;;) {

    printf("waiting for UDP datagram.\n");

    char line[1024];
    int len = recvfrom(fd, line, sizeof(line), 0, (struct sockaddr *) 0, NULL);

    if (len < 0)
      fatalerror("recvfrom failed.");

    drive(line);
    
  }

#endif
}

////////////////////////////////////////////////////////////////////////////////

int 
main(int argc, char *argv[])
{

  if (argc != 2)
      fatalerror("invalid command-line arguments.");
  
  gpiostart();
#if OAXACA_TM || OAXACA_DM
  dacstart();
#endif

  if (strcmp(argv[1], "server") == 0) {
    server();
  } else if (strcmp(argv[1], "local") == 0) {
    local();
  } else {
    fatalerror("invalid command-line arguments.");
  }

  return 0;
}
